#include "pch.h"
#include "VolumeConstraint.h"
#include "PositionBasedDynamics.h"

bool CVolumeConstraint::initializeConstraint(CParticleData& vParticleData, const std::vector<unsigned int>& vParticles, double vStiffness)
{
	m_stiffness = vStiffness;
	int ParticleCount = vParticles.size();
	_ASSERTE(ParticleCount == 4);

	std::vector<Eigen::Vector3d> Pos(ParticleCount);
	for (int i = 0; i < ParticleCount; i++)
	{
		m_ParticleIndices[i] = vParticles[i];
		Pos[i] = vParticleData.getPosition0(vParticles[i]);
	}

	m_restVolume = fabs(static_cast<double>(1.0 / 6.0) * (Pos[3] - Pos[0]).dot((Pos[2] - Pos[0]).cross(Pos[1] - Pos[0])));

	return true;
}

bool CVolumeConstraint::solveConstraintV(CParticleData& vParticleData)
{
	const unsigned i1 = m_ParticleIndices[0];
	const unsigned i2 = m_ParticleIndices[1];
	const unsigned i3 = m_ParticleIndices[2];
	const unsigned i4 = m_ParticleIndices[3];

	Eigen::Vector3d &x1 = vParticleData.getPosition(i1);
	Eigen::Vector3d &x2 = vParticleData.getPosition(i2);
	Eigen::Vector3d &x3 = vParticleData.getPosition(i3);
	Eigen::Vector3d &x4 = vParticleData.getPosition(i4);

	const double invMass1 = vParticleData.getInvMass(i1);
	const double invMass2 = vParticleData.getInvMass(i2);
	const double invMass3 = vParticleData.getInvMass(i3);
	const double invMass4 = vParticleData.getInvMass(i4);

	Eigen::Vector3d corr1, corr2, corr3, corr4;
	const bool res = CPositionBasedDynamics::solve_VolumeConstraint(x1, invMass1,
		x2, invMass2,
		x3, invMass3,
		x4, invMass4,
		m_restVolume,
		m_stiffness,
		m_stiffness,
		corr1, corr2, corr3, corr4);

	_ASSERTE(res == true);
	if (res)
	{
		if (invMass1 != 0.0)
			x1 += corr1;
		if (invMass2 != 0.0)
			x2 += corr2;
		if (invMass3 != 0.0)
			x3 += corr3;
		if (invMass4 != 0.0)
			x4 += corr4;
	}
	return res;
}
